#include "general_ctrl/PositionPID.hpp"

namespace GeneralCtrl
{
PositionPID::PositionPID(double P, double I, double D)
{
    SetP(P);
    SetI(I);
    SetD(D);
}

void PositionPID::SetP(double value)
{
    P = value;
}

void PositionPID::SetI(double value)
{
    I = value;
}

void PositionPID::SetD(double value)
{
    D = value;
}

void PositionPID::SetPID(double p, double i, double d)
{
    P = p;
    I = i;
    D = d;
}

double PositionPID::GetControl(double error, double timeDelta)
{
    errorSum += error * timeDelta;
    double derror = (error - lastError) / timeDelta;
    double ctrl = P * error + I * errorSum + D * derror;
    lastError = error;
    return ctrl;
}
} // namespace GeneralCtrl
